#include "trace.h"
#include "slep.h"


int rankofMat(double* A,int m, int n){
	double *sing = (double*) malloc( sizeof(double) * minof2(m,n));
	double *Acopy = (double *) malloc(sizeof(double) * m * n );
	int rank1,i, info;
	dcopy(m*n,A,1,Acopy,1);
	dgesvd('N','N',m,n,Acopy,m,sing,NULL,1,NULL,1,&info);
	rank1=0;
	for(i=0;i< minof2(m,n);i++)
		rank1 += ((sing[i]>1e-7) ? 1:0);
	free(sing);
	free(Acopy);
	return rank1;
}


void setTRACEOPTStol(double tol, TRACEOPTS* opt){
	if(strstr(opt->registry,"tol")== NULL )
		strcat(opt->registry," tol");
	opt->tol = tol;
}

void setTRACEOPTSfreq(double freq, TRACEOPTS* opt){
	if(strstr(opt->registry,"freq")== NULL )
		strcat(opt->registry," freq");
	opt->freq = freq;
}

void dcopyMatrixTranspose(int m,int n, double* A, int lda, double *B, int ldb){
	int i,j;
	for(j=0;j<n;j++)
		for(i=0;i<m;i++)
			B[ldb*i + j]=A[j*lda + i];
	return;
}

/*
* Takes A - m*n, and trans in {'T' or 'N'} and stores the result in given q & r (q-m*m & r-m*n if trans='N' else q-n*n & r-n*m)
*/
void slepQRFactorize(char trans,int m, int n,double *a, double *q, double *r){
	int info, i;
	double *tau;
	tau = (double *) malloc( sizeof(double) * minof2(m,n) );
	if(trans=='T' || trans == 't'){
		dcopyMatrixTranspose(m,n,a,m,r,n);
		dgeqrf(n,m,r,n,tau,&info);
		memset(q,0,sizeof(double) * n * n);
		for(i=0;i<m;i++){
			if(i< minof2(m,n)){
				dcopy(n-(i+1),&r[i*n+i+1],1,&q[i*n+i+1],1);
				memset(&r[i*n+i+1],0,(n-(i+1))*sizeof(double));
			}
		}
		dorgqr(n,minof2(n,m),minof2(n,m),q,n,tau,&info);
	}else if(trans == 'N' || trans=='n'){
		dcopy(m*n,a,1,r,1);
		dgeqrf(m,n,r,m,tau,&info);
		memset(q,0,sizeof(double) * m * m);
		for(i=0;i<n;i++){
			if(i< minof2(m,n)){
				dcopy(m-(i+1),&r[i*m+i+1],1,&q[i*m+i+1],1);
				memset(&r[i*m+i+1],0,(m-(i+1))*sizeof(double));
			}
		}
		dorgqr(m,minof2(m,n),minof2(m,n),q,m,tau,&info);
	}
	free (tau);
}
/* Assumes lda=n */
void slepInvert(int n,double *A){
	int *ipiv = (int*) malloc(sizeof(int) * n);
	int info;
	dgetrf(n,n,A,n,ipiv,&info);
	dgetri(n,A,n,ipiv,&info);
	free (ipiv);
}

int largestMagnitude(int n, double *wReal,double *wImag){
	int i, index=0;
	for(i=0;i<n;i++)
		if((wReal[i]*wReal[i]+wImag[i]*wImag[i]) > (wReal[index]*wReal[index]+wImag[i]*wImag[index]) )
			index=i;
	return index;
}


// Assumes LDA=m U-m*min(m,n) VT-min(m,n)*n sing-min(m,n)
void slepSVD(int m, int n, double *A, int lda, double* sing, double *U,int ldu, double *VT, int ldvt){
	int info;
	double *B;
	B = (double *) malloc( sizeof(double) * m * n);
	dcopy(m*n,A,1,B,1);
	dgesvd('S','S',m,n,B,lda,sing,U,ldu,VT,ldvt,&info);
	free(B);
}

double myTrace2(char transa,char transb, int m, int n, double *A, int lda, double *B, int ldb){
	double trace=0;
	int i;
	if(transa=='N' || transa== 'n'){
		if(transb=='N' || transb== 'n')
			for(i=0;i<m;i++)
				trace += ddot(n,&A[i],lda,&B[i*ldb],1);
		else if(transb=='T' || transb== 't')
			for(i=0;i<m;i++)
				trace += ddot(n,&A[i],lda,&B[i],ldb);
	}
	else if(transa=='T' || transa== 't'){
		if(transb=='N' || transb== 'n')
			for(i=0;i<m;i++)
				trace += ddot(n,&A[i*lda],1,&B[i*ldb],1);
		else if(transb=='T' || transb== 't')
			for(i=0;i<m;i++)
				trace += ddot(n,&A[i*lda],1,&B[i],ldb);
	}
	return trace;
}


/* 
* A is m*n B is n*k, C is k*m so final square matrix will be m*m whose trace is returned
*/
double myTrace3(char transa, int m, int n, int k, double* A, int lda, double* B, int ldb, double* C, int ldc){
	double *tmp;
	double trace;
	tmp = (double *) malloc( sizeof(double) * n*m);
	dgemm('N','N',n,m,k,1,B,ldb,C,ldc,0,tmp,n);
	trace = myTrace2(transa,'N',m,n,A,lda,tmp,n);
	free(tmp);
	return trace;
}



double slepNuclearNorm(int m, int n, double *A, int lda){
	double *sing,   norm=0;
	int i, info, k=minof2(m,n);
	sing = (double *)malloc(sizeof(double)*k );
	dgesvd('N','N',m,n,A,lda,sing,NULL,1,NULL,1,&info);
	for (i=0; i<k;i++)
		norm += sing[i];
	free(sing);
	return norm;
}